#include <stdio.h>
#include "../../include/sensors/lsm303DLH_a.h"

/* TODO vérifier ce qu'il y a à "reseter" */
void reset_values_acc(acc_sensor *acc)
{
    /* TODO: vérifier que les données sont en accord avec data constructeur */
	acc->calibration.bias[0] = 0;
	acc->calibration.bias[1] = 0;
	acc->calibration.bias[2] = 0;
   	acc->calibration.A_1[0] = 0;
	acc->calibration.A_1[1] = 0;
	acc->calibration.A_1[2] = 0;
	acc->calibration.A_1[3] = 0;
	acc->calibration.A_1[4] = 0;
	acc->calibration.A_1[5] = 0;

}

int Acc_Init(acc_sensor *acc){
    FILE *facc;
    int errno;
    facc = fopen(ACC_Name_file, "r"); /* TODO attention il s'agit du nom pas d'ID de dispo... peut etre le meme que accéléromètre */
    if(facc < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fscanf(facc, "%s",acc->name);
    fclose(facc);
    printf("sensor name: %s\n",acc->name);
    /* TODO récupérer ID du accéléromètre.. */

    facc = fopen(ACC_Mode_file, "w+");
    if(facc < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fprintf(facc,"%d",LSM303DLH_A_MODE_NORMAL);
    fclose(facc);

    acc->config.mode=LSM303DLH_A_MODE_NORMAL;
/*
    facc = fopen(ACC_Rate_file, "r");
    if(facc < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fscanf(facc, "%x",&acc->config.rate);
    fclose(facc);
*/
    facc = fopen(ACC_Range_file, "r");
    if(facc < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fscanf(facc, "%d",&acc->config.range);
    fclose(facc);

    return 0;
}

/* structure de donées pour la configuration du MEMs (rate, mode, ...) */
/* TODO revoir les IF sans doute il faut mettre des else */
int Acc_write_config(acc_sensor *acc)
{
    FILE *facc;
    int errno;
    facc = fopen(ACC_Mode_file, "w+");
    if(facc < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fprintf(facc,"%d",acc->config.mode);
    fclose(facc);
/*
    facc = fopen(ACC_Rate_file, "w+");
    if(facc < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fprintf(facc,"%d",acc->config.rate);
    fclose(facc);
*/
    facc = fopen(ACC_Range_file, "w+");
    if(facc < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fprintf(facc,"%d",acc->config.range);
    fclose(facc);
    /* pas besoin d'écrire le gain il se met automatiquement à jour (normalement) */
    return 0;
}

int Acc_read_config(acc_sensor *acc)
{
    FILE *facc;
    int errno;
    facc = fopen(ACC_Name_file, "r");
    if(facc < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fscanf(facc, "%s",acc->name);
    printf("sensor name: %s\n",acc->name);
    fclose(facc);

    /* TODO: ajouter lecture de l'ID */

    facc = fopen(ACC_Mode_file, "r");
    if(facc < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(facc, "%d",&acc->config.mode))<1)
        printf("failed to read sensor rate");
    fclose(facc);

    facc = fopen(ACC_Range_file, "r");
    if(facc < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(facc, "%d",&acc->config.range))<1)
        printf("failed to read sensor range");
    fclose(facc);
/*
    facc = fopen(ACC_Rate_file, "r");
    if(facc < 0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    if((fscanf(facc, "%d",&acc->config.rate))<1)
        printf("failed to read sensor rate");
    fclose(facc);
*/
    return 0;
}

/*TODO fonction de lecture des données du accnétomètre */
/* verifier que la data est bien dispo sinon en renvoie la même donnée
    peut se faire sans doute avec IRQ, à étudier... */
int Acc_read_data(acc_sensor *acc)
{
    int errno,ret;
    int X,Y,Z;
    FILE *facc;
    /* verifier que le accnétomètre est en marche ??? ou faire dans fonction au dessus... */
    facc = fopen(ACC_Data_file, "r");
    if(facc<0)
    {
        printf("failed to open file, errno %d\n", errno);
        return -1;
    }
    fseek(facc, 0, SEEK_SET);
    ret = fscanf(facc, "%x:%x:%x", &X,&Y,&Z);//&acc->raw.axis[0], &acc->raw.axis[1], &acc->raw.axis[2]);
    if (ret<3)
    {
    	printf("failed to read data acc");
    }
    else
    {
    	acc->raw.axis[0] = X; /* TODO: verifier la formule ... */
        acc->raw.axis[1] = Y;
        acc->raw.axis[2] = Z;
        /*
        printf("acc data x: %x\t",acc->raw.axis[0]);
        printf("acc data y: %x\t",acc->raw.axis[1]);
        printf("acc data z: %x\n",acc->raw.axis[2]);
        */
    }
    fclose(facc);
    return 0;
}
